/*
 * Copyright The Slam Authors
 */

#ifndef SLAM_MAPPING_INTERNAL_2D_CHECK_LOOP_CONSTRAINTS_H_
#define SLAM_MAPPING_INTERNAL_2D_CHECK_LOOP_CONSTRAINTS_H_

#include <vector>

#include "slam/mapping/2d/probability_grid.h"
#include "slam/mapping/2d/submap_2d.h"
#include "slam/mapping/id.h"
#include "slam/mapping/internal/optimization/optimization_problem_2d.h"
#include "slam/mapping/pose_graph_data.h"
#include "slam/sensor/point_cloud.h"
#include "slam/transform/rigid_transform.h"
#include "slam/transform/transform.h"

namespace slam {
namespace mapping {

class CheckLoopConstraints {
 public:
  CheckLoopConstraints(const int submap_interval,
                       const float max_delta_translation,
                       const float max_delta_rotation)
      : submap_interval_(submap_interval),
        max_delta_translation_(max_delta_translation),
        max_delta_rotation_(max_delta_rotation) {}

  ~CheckLoopConstraints() {}

  bool DeleteErrorConstraint(
      PoseGraphData* pose_graph_data,
      const optimization::OptimizationProblem2D* const optimization_problem);

  void SetCurrentTrajectoryId(const int& trajectory_id);

  void BackupOptimizationData(
      const optimization::OptimizationProblem2D* const optimization_problem);

  void ResetOptimizationData(
      optimization::OptimizationProblem2D* const optimization_problem);

  void Reset();

 private:
  std::map<NodeId, optimization::NodeSpec2D> node_data_bak_;
  std::map<SubmapId, optimization::SubmapSpec2D> submap_data_bak_;
  std::map<std::string, transform::Rigid3d> landmark_data_bak_;

  const int submap_interval_;
  const float max_delta_translation_;
  const float max_delta_rotation_;
  int current_trajectory_id_ = 0;
};

}  // namespace mapping
}  // namespace slam

#endif